import roslib
import rospy
import socket
import time
import math
import numpy
from ackermann_msgs.msg import AckermannDrive

test_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime())
target_x = 1
target_y = 0
max_angle = 0.5    # the max angle is 0.5
max_speed = 1    # the max speed is 1
switch = {0: case0,
        1: case1,
        2: case2,
        3: case3,
        4: case4,
        5: case5,
        6: case6,
        7: case7,
        }
ackermann = AckermannDrive()
ackermann.speed = 0.0
ackermann.steering_angle = 0.0
ackermann_cmd_pub = rospy.Publisher('/tianracer/ackermann_cmd', AckermannDrive, queue_size=5)
s = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
s.connect(("127.0.0.1", 6666))

def get_once_position():
    """
    Get the position of the vehicle in front relative to this vehicle
    """
    msgs = s.recv(1000).replace('(', '').replace(')', '').replace(' ', '').split(',') 
    x = float(msgs[4])
    y = float(msgs[2])
    return x, y

def is_zero(test_number, max_number, tolerance):
    """
    The speed close to zero will be thought as zero
    """
    if abs(test_number)/max_number < tolerance:
        return True
    else:
        return False

def get_positon_and_towards():
    """
    Use two marks to determine the front vehicle's position and towards
    """
    x1, y1 = get_once_position()
    x2, y2 = get_once_position()
    x = (x1 + x2) / 2
    y = (y1 + y2) / 2
    dx = x2 - x1
    dy = y2 - y1
    theda = atan(dy/dx)
    return x, y, theda

def get_command(x, y, theda):
    """
    Divide all cases into eight types
    Depend on x, y and theda
    """
    x_is_target = not is_zero(x-target_x, 2, 0.1)
    y_is_target = not is_zero(y-target_y, 2, 0.1)
    theda_is_target = not is_zero(theda, 1.5, 0.1)
    case = x_is_target*4 + y_is_target*2 + theda_is_target
    return case

def case0(x, y, theda):
    """
    get the target
    """
    speed = 0
    angle = 0
    ackermann.speed = speed
    ackermann.steering_angle = angle
    ackermann_cmd_pub.publish(ackermann)

def case1(x, y, theda):
    """
    docstring
    """

def case2(x, y, theda):
    """
    docstring
    """

def case3(x, y, theda):
    """
    docstring
    """

def case4(x, y, theda):
    """
    docstring
    """

def case5(x, y, theda):
    """
    docstring
    """

def case6(x, y, theda):
    """
    docstring
    """

def case7(x, y, theda):
    """
    docstring
    """

if __name__ == "__main__":
    rospy.init_node('ros_talker')
    cnt = 0
    T = 20
    r = rospy.Rate(T)
    f = open('/home/tianbot/tianbot_ws/src/tianracer/tianracer_test/scripts/test.txt','a')
    f.write('\n%s' % test_time)
    try:
        #f.write('\n%s' % test_time)
        while not rospy.is_shutdown():
            x, y, theda = get_positon_and_towards()
            case = get_command(x, y, theda)
            switch.get(case, case0)()
            cnt += 1
            print(cnt,x,y,theda)
            f.write('\nx, y, theda: %s, %s, %s' % (x, y, theda))
            

    except Exception as e:
        print(e)

    finally:
        ackermann = AckermannDrive()
        ackermann.speed = 0.0
        ackermann.steering_angle = 0.0
        ackermann_cmd_pub.publish(ackermann)
        s.close()
        f.close()